#!/usr/bin/python2.7
# coding=UTF-8
import rospy
from mavros_msgs.msg import GlobalPositionTarget, State, PositionTarget, AttitudeTarget
from mavros_msgs.srv import CommandBool, CommandTOL, SetMode
from geometry_msgs.msg import PoseStamped, Twist, Pose, Point, Vector3Stamped, Quaternion, TwistStamped
from trajectory_msgs.msg import JointTrajectoryPoint
from sensor_msgs.msg import Imu, NavSatFix
from std_msgs.msg import Float32, Float64, String, Bool
from tf.transformations import quaternion_from_euler

import time
import math

import numpy as np

import pid_controller  # use pid_control


# PX4 控制器
class Px4Controller:
    def __init__(self):
        # uav sensor state
        self.arm_state = False
        self.offboard_state = False
        self.mavros_state = State()

        # uav state
        self.state = State()
        self.local_pose = PoseStamped()  # state of uav ( The origin is the takeoff point )
        self.time = rospy.get_time()
        self.current_state = PoseStamped()  # position of uav
        self.velocity_state = [0, 0, 0]  # velocity of uav
        self.current_heading = 0.0  # current heading(yaw)

        # target_state
        self.takeoff_height = 1.5  # target height
        self.target_position_initial = [0, 0, 0]  # target position initial
        self.target_yaw = 0.0  # target yaw initial
        self.target_position_cur = [0, 0, 0]  # current target position
        self.forward_error = 100  # forward error
        self.cur_target_state = AttitudeTarget()  # publish current target -- Attitude Target()

        # obstacle state
        self.plan_start_flag = False
        self.plan_end_state = PoseStamped()
        self.obstacle_position_cur = [0.0, 0.0, 0.0]
        self.obstacle_velocity_cur = [0.0, 0.0, 0.0]  # current target velocity
        self.obstacle_acceleration_cur = [0.0, 0.0, 0.0]  # current target acceleration
        self.obstacle_yaw_cur = 0.0
        self.obstacle_fly_time = 2

        # land point set
        self.end_point = [0.0, 0.0, 0.0]

        # control param
        self.control_rate = 30
        self.reserved_distance = [2.6, 3, 2.6, 3, 3, 3, 2.6]
        self.cross_distance = [4, 4, 4, 4, 5, 5, 3.4]
        self.corner_distance = [1]
        self.tol = 0.04

        # ros initial
        rospy.init_node('uav_control_node')
        '''
        ros service
        '''
        self.armService = rospy.ServiceProxy("/mavros/cmd/arming", CommandBool)  # Change Arming status
        self.flightModeService = rospy.ServiceProxy("/mavros/set_mode", SetMode)  # Set FCU operation mode
        '''
        ros subscribe
        '''
        self.uav_local_pose_sub = rospy.Subscriber("/mavros/local_position/pose", PoseStamped,
                                                   self.uav_local_pose_callback)  # get uav state
        # self.uav_local_pose_sub = rospy.Subscriber("/mavros/local_position/pose", PoseStamped,
        #                                            self.uav_local_pose_callback)  # get uav state
        self.uav_local_velocity_sub = rospy.Subscriber('/mavros/local_position/velocity_local', TwistStamped,
                                                       self.uav_local_velocity_callback)  # get uav velocity
        self.mavros_sub = rospy.Subscriber("/mavros/state", State, self.mavros_state_callback)
        self.obstacle_sub = rospy.Subscriber("/pva_setpoint", JointTrajectoryPoint, self.obstacle_callback)
        '''
        ros publish
        '''
        # self.local_target_p_pub = rospy.Publisher("/mavros/setpoint_position/local", PoseStamped, queue_size=1)
        self.local_target_p_pub = rospy.Publisher("/mavros/setpoint_raw/local", PositionTarget, queue_size=1)
        self.attitude_target_pub = rospy.Publisher("/mavros/setpoint_raw/attitude", AttitudeTarget, queue_size=1)
        self.plan_start_pub = rospy.Publisher("/plan/start", Bool, queue_size=1)
        self.plan_end_point_pub = rospy.Publisher("/plan/end_point", PoseStamped, queue_size=1)
        print("Px4 Controller Initialized!")

    # <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< ros callback function >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
    # get uav local pose
    def uav_local_pose_callback(self, msg):
        self.local_pose = msg
        self.current_state = msg
        self.time = msg.header.stamp.secs
        self.current_heading = self.q2yaw(msg.pose.orientation.w, msg.pose.orientation.x, msg.pose.orientation.y,
                                          msg.pose.orientation.z)
        if self.current_heading < 0:
            # self.current_heading = self.current_heading + 2 * np.pi
            self.current_heading = self.current_heading 

            # self.current_heading = self.current_heading 

            # self.current_heading = self.current_heading 

        # print ('yaw', self.current_heading)
        # print ('call back pose', self.current_state.pose.position)

    # get mavros state
    def mavros_state_callback(self, msg):
        self.mavros_state = msg.mode
        self.state = msg

    # get velocity state
    def uav_local_velocity_callback(self, msg):
        self.velocity_state = [msg.twist.linear.x, msg.twist.linear.y, msg.twist.linear.z]

    # obstacle callback
    def obstacle_callback(self, msg):
        self.obstacle_position_cur = [msg.positions[0], msg.positions[1], msg.positions[2]]
        self.obstacle_yaw_cur = msg.positions[3]
        self.obstacle_velocity_cur = msg.velocities
        self.obstacle_acceleration_cur = msg.accelerations

    # <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< task function >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
    # set point to avoid obstacle (use setpoint_raw method)
    def set_obstacle_point(self):
        pid = pid_controller.PID_controller()
        pid.init_time(rospy.get_time())
        r = rospy.Rate(self.control_rate)
        r.sleep()

        self.obstacle_position_cur=[self.current_state.pose.position.x,self.current_state.pose.position.y,self.current_state.pose.position.z]
        self.obstacle_velocity_cur=[0,0,0]
        self.obstacle_acceleration_cur=[0,0,0]

        while not rospy.is_shutdown():
            cur_x = self.current_state.pose.position.x
            cur_y = self.current_state.pose.position.y
            cur_z = self.current_state.pose.position.z
            time_now = rospy.get_time()
            # print("time: ",time_now)

            # print("obstacle_position_cur: ",self.obstacle_position_cur)
            # print("obstacle_velocity_cur: ",self.obstacle_velocity_cur)
            # print("obstacle_acceleration_cur: ",self.obstacle_acceleration_cur)

            roll, pitch, thrust = pid.set_pva_in_attitude(target_p=self.obstacle_position_cur,
                                                          target_v=self.obstacle_velocity_cur,
                                                          target_a=self.obstacle_acceleration_cur,
                                                          cur_p=[cur_x, cur_y, cur_z],
                                                          cur_velocity=self.velocity_state,
                                                          time=time_now,
                                                          yaw=self.current_heading)
            q = self.RPY2Quar(roll, pitch, self.obstacle_yaw_cur)
            self.cur_target_state = self.construct_target_AttitudeTarget(q, thrust)
            self.attitude_target_pub.publish(self.cur_target_state)
            target_x = self.end_point[0]
            target_y = self.end_point[1]
            target_z = self.end_point[2]
            break_flag = self.cal_pv_fly_error(self.current_state,
                                               [target_x, target_y, target_z],
                                               yaw=0.0,
                                               threshold=self.tol)
            if break_flag:
                break
            r.sleep()


    # <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< fly function >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
    # set point to fly  (use setpoint_raw method)
    def set_point(self, target_x, target_y, target_z, yaw=0, tol=0.05):
        while not rospy.is_shutdown():
            self.cur_target_state = self.construct_target_PositionTarget(x=target_x,
                                                                         y=target_y,
                                                                         z=target_z,
                                                                         yaw=yaw,
                                                                         frame="local_frame",
                                                                         )  # Here, yaw = pi / 2 is regarded as the direction of the ENU coordinate system
            # x,y,z or vx,vy,vz can be used
            self.local_target_p_pub.publish(self.cur_target_state)
            # tol = 0.0005  # tolerance
            break_flag = self.position_distance(self.current_state, self.cur_target_state, tol)
            if break_flag:
                break

    # set point to fly (use roll pitch yaw thrust)
    def set_point_in_attitude(self, target_x, target_y, target_z, yaw=0, tol=0.1, set_time=2):
        pid = pid_controller.PID_controller()
        pid.init_time(rospy.get_time())
        r = rospy.Rate(self.control_rate)
        r.sleep()
        k = 0
        while not rospy.is_shutdown():
            cur_x = self.current_state.pose.position.x
            cur_y = self.current_state.pose.position.y
            cur_z = self.current_state.pose.position.z
            # yaw = self.current_heading
            time_now = rospy.get_time()
            roll, pitch, thrust = pid.set_point_in_attitude(target_x, target_y, target_z,
                                                            cur_x, cur_y, cur_z,
                                                            self.velocity_state,
                                                            time_now, self.current_heading)
            # roll = 0.2
            # pitch = 0
            # yaw = np.pi/2
            # thrust = 0.5644

            q = self.RPY2Quar(roll, pitch, yaw)
            self.cur_target_state = self.construct_target_AttitudeTarget(q, thrust)

            self.attitude_target_pub.publish(self.cur_target_state)
            break_flag = self.cal_error([cur_x, cur_y, cur_z], [target_x, target_y, target_z], tol)
            if break_flag:
                break
            r = rospy.Rate(self.control_rate)
            r.sleep()
            k = k + 1
            if k > set_time * self.control_rate:
                break

    # set point to fly (use roll pitch yaw thrust)
    def set_point_slow_yaw_in_attitude(self, target_x, target_y, target_z, yaw=0, tol=0.01, set_time=5):
        pid = pid_controller.PID_controller()
        pid.init_time(rospy.get_time())
        r = rospy.Rate(self.control_rate)
        r.sleep()
        k = 0
        yaw_start = self.current_heading
        yaw_pub = rospy.Publisher('/rotate_yaw', Float32, queue_size=10)


        end_point_x = self.end_point[0]
        end_point_y = self.end_point[1]
        end_point_z = self.end_point[2]

        while not rospy.is_shutdown():

            self.plan_end_state = self.construct_target_PoseStamped(end_point_x, end_point_y, end_point_z)
            print ('end point', self.end_point)
            self.plan_end_point_pub.publish(self.plan_end_state)


            cur_x = self.current_state.pose.position.x
            cur_y = self.current_state.pose.position.y
            cur_z = self.current_state.pose.position.z
            # yaw = self.current_heading
            time_now = rospy.get_time()
            roll, pitch, thrust = pid.set_point_in_attitude(target_x, target_y, target_z,
                                                            cur_x, cur_y, cur_z,
                                                            self.velocity_state,
                                                            time_now, self.current_heading)
            yaw_temp = yaw_start + (yaw-yaw_start)/(set_time * self.control_rate) * k
            yaw_msg = Float32()
            yaw_msg.data = self.current_heading
            yaw_pub.publish(yaw_msg)
            # if yaw satisfy target yaw
            if abs(self.current_heading - yaw) < 0.05 or abs(self.current_heading - yaw) > 2*np.pi - 0.1:
                print ('yaw arrive')
                print ('self yaw', self.current_heading)
                print ('target yaw', yaw)
                break

            # if time is out
            if k > set_time * self.control_rate:
                print ('time is out', [cur_x, cur_y, cur_z])
                break
            q = self.RPY2Quar(roll, pitch, yaw_temp)
            self.cur_target_state = self.construct_target_AttitudeTarget(q, thrust)

            self.attitude_target_pub.publish(self.cur_target_state)
            r = rospy.Rate(self.control_rate)
            r.sleep()
            k = k + 1

    def take_off(self):
        pid = pid_controller.PID_controller()
        pid.init_time(rospy.get_time())
        r = rospy.Rate(self.control_rate)
        r.sleep()
        # set take off point
        target_x = self.current_state.pose.position.x
        target_y = self.current_state.pose.position.y
        target_z = self.current_state.pose.position.z
        cur_x = self.current_state.pose.position.x
        cur_y = self.current_state.pose.position.y
        cur_z = self.current_state.pose.position.z
        yaw = self.current_heading
        time_now = rospy.get_time()
        roll, pitch, thrust = pid.set_point_in_attitude(target_x, target_y, target_z,
                                                        cur_x, cur_y, cur_z,
                                                        self.velocity_state,
                                                        time_now, yaw)
        q = self.RPY2Quar(roll, pitch, yaw)
        self.cur_target_state = self.construct_target_AttitudeTarget(q, thrust)

        # self.arm()
        # self.offboard()

        # if uav is not connected
        while (not rospy.is_shutdown()) and self.state.connected == False:
            print ("wait for connected")
            r = rospy.Rate(self.control_rate)
            r.sleep()

        # wait publish data
        for i in range(15):
            self.attitude_target_pub.publish(self.cur_target_state)  # take off
            r = rospy.Rate(self.control_rate)
            r.sleep()

        # if it is not offboard
        while (not rospy.is_shutdown()):
            print ("wait for offboard or armed")
            # set take off point
            target_x = self.current_state.pose.position.x
            target_y = self.current_state.pose.position.y
            target_z = self.takeoff_height
            cur_x = self.current_state.pose.position.x
            cur_y = self.current_state.pose.position.y
            cur_z = self.current_state.pose.position.z
            yaw = self.current_heading
            time_now = rospy.get_time()
            roll, pitch, thrust = pid.set_point_in_attitude(target_x, target_y, target_z,
                                                            cur_x, cur_y, cur_z,
                                                            self.velocity_state,
                                                            time_now, yaw)
            q = self.RPY2Quar(roll, pitch, yaw)
            self.cur_target_state = self.construct_target_AttitudeTarget(q, thrust)
            self.attitude_target_pub.publish(self.cur_target_state)  # take off
            r = rospy.Rate(self.control_rate)
            r.sleep()
            if self.state.armed and self.state.mode == "OFFBOARD":
                break

        print ("yaw start", self.current_heading)
        print('---------------------------------------')

        # set the last yaw when take off
        target_x = self.current_state.pose.position.x
        target_y = self.current_state.pose.position.y
        for i in range(10):
            target_z = self.takeoff_height / 10 * (i + 1)
            cur_x = self.current_state.pose.position.x
            cur_y = self.current_state.pose.position.y
            cur_z = self.current_state.pose.position.z
            time_now = rospy.get_time()
            roll, pitch, thrust = pid.set_point_in_attitude(target_x, target_y, target_z,
                                                            cur_x, cur_y, cur_z,
                                                            self.velocity_state,
                                                            time_now, yaw)
            # print('rqyt',[roll,pitch,yaw,thrust])
            q = self.RPY2Quar(roll, pitch, yaw)
            self.cur_target_state = self.construct_target_AttitudeTarget(q, thrust)
            self.attitude_target_pub.publish(self.cur_target_state)
            r = rospy.Rate(self.control_rate)
            r.sleep()
            if rospy.is_shutdown():
                break

        # set the last yaw when take off
        target_x = self.current_state.pose.position.x
        target_y = self.current_state.pose.position.y
        target_z = self.takeoff_height
        for i in range(self.control_rate/2):
            cur_x = self.current_state.pose.position.x
            cur_y = self.current_state.pose.position.y
            cur_z = self.current_state.pose.position.z
            time_now = rospy.get_time()
            roll, pitch, thrust = pid.set_point_in_attitude(target_x, target_y, target_z,
                                                            cur_x, cur_y, cur_z,
                                                            self.velocity_state,
                                                            time_now, yaw)
            # print('rqyt',[roll,pitch,yaw,thrust])
            q = self.RPY2Quar(roll, pitch, yaw)
            self.cur_target_state = self.construct_target_AttitudeTarget(q, thrust)
            self.attitude_target_pub.publish(self.cur_target_state)
            r = rospy.Rate(self.control_rate)
            r.sleep()
            if rospy.is_shutdown():
                break

        if self.takeoff_detection():
            print("Vehicle Took Off!")
        else:
            print("Vehicle Took Off Failed!")
            return

    def land(self):
        cur_x = self.current_state.pose.position.x
        cur_y = self.current_state.pose.position.y
        cur_z = self.current_state.pose.position.z

        yaw = self.current_heading
        k = 0
        r = rospy.Rate(self.control_rate)

        while not rospy.is_shutdown():
            land_v=1.0
            self.cur_target_state = self.construct_target_PositionTarget(x=cur_x,
                                                                         y=cur_y,
                                                                         z=cur_z-land_v*1.0/self.control_rate*k,
                                                                         yaw=yaw,
                                                                         frame="local_frame",
                                                                         )  # Here, yaw = pi / 2 is regarded as the direction of the ENU coordinate system
            # x,y,z or vx,vy,vz can be used
            self.local_target_p_pub.publish(self.cur_target_state)
            # tol = 0.0005  # tolerance
            k = k +1
            if k > (cur_z+2.0)/land_v*self.control_rate:
                self.disarm()
                break
            r.sleep()

    def land_in_attitude(self):
        # set take off point
        pid = pid_controller.PID_controller()
        pid.init_time(rospy.get_time())
        r = rospy.Rate(self.control_rate)
        r.sleep()
        k = 0
        target_x = self.current_state.pose.position.x
        target_y = self.current_state.pose.position.y
        land_initial_z = self.current_state.pose.position.z
        yaw = self.current_heading
        while not rospy.is_shutdown():
            # set land point
            target_z = (land_initial_z + 3) * (1 - 1 / 100 * k) - 3
    
            cur_x = self.current_state.pose.position.x
            cur_y = self.current_state.pose.position.y
            cur_z = self.current_state.pose.position.z
            time_now = rospy.get_time()
            roll, pitch, thrust = pid.set_point_in_attitude(target_x, target_y, target_z,
                                                            cur_x, cur_y, cur_z,
                                                            self.velocity_state,
                                                            time_now, yaw)
            q = self.RPY2Quar(roll, pitch, yaw)
            self.cur_target_state = self.construct_target_AttitudeTarget(q, thrust)
            k = k + 1
            # x,y,z or vx,vy,vz can be used
            self.attitude_target_pub.publish(self.cur_target_state)
            # tol = 0.0005  # tolerance
            if  k > 100:
                self.disarm()
                break

    def hover(self):
        cur_x = self.current_state.pose.position.x
        cur_y = self.current_state.pose.position.y
        cur_z = self.current_state.pose.position.z
        yaw = self.target_yaw
        while not rospy.is_shutdown():
            self.cur_target_pose = self.construct_target_PositionTarget(x=cur_x,
                                                                        y=cur_y,
                                                                        z=cur_z,
                                                                        yaw=yaw)
            # x,y,z or vx,vy,vz can be used
            self.local_target_p_pub.publish(self.cur_target_state)

    # <<<<<<<<<<<<<<<<<<<<<<<<<<<<< Switch safe and controllable mode function >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
    def arm(self):
        if self.armService(True):
            return True
        else:
            print("Vehicle arming failed!")  # print out when failed
            return False

    def disarm(self):
        if self.armService(False):
            return True
        else:
            print("Vehicle disarming failed!")  # print out when failed
            return False

    def offboard(self):
        if self.flightModeService(custom_mode='OFFBOARD'):
            return True
        else:
            print("Vechile Offboard failed")  # print out when failed
            return False

    def takeoff_detection(self):
        # when the uav take off to 0.1m and in safe state
        if self.current_state.pose.position.z > 0.1 and self.offboard_state and self.arm_state:
            return True
        else:
            return False

    def land_detection(self):
        # when the uav take off to 0.1m and in safe state
        if self.current_state.pose.position.z < -0.1 and self.offboard_state and self.arm_state:
            return True
        else:
            return False

    # <<<<<<<<<<<<<<<<<<<<<<<<<<<< coordinate system conversion function >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
    # For Body frame, we will use FLU (Forward, Left and Up)
    #           +Z     +X
    #            ^    ^
    #            |  /
    #            |/
    #  +Y <------body
    # For world frame, we will use ENU (EAST, NORTH and UP)
    #     +Z     +Y
    #      ^    ^
    #      |  /
    #      |/
    #    world------> +X
    #  FLU -> ENU
    def FLU2ENU(self, msg):
        FLU_x = msg.pose.position.x * math.cos(self.current_heading) - msg.pose.position.y * math.sin(
            self.current_heading)
        FLU_y = msg.pose.position.x * math.sin(self.current_heading) + msg.pose.position.y * math.cos(
            self.current_heading)
        FLU_z = msg.pose.position.z
        return FLU_x, FLU_y, FLU_z

    #  ENU -> FLU
    def ENU2FLU(self, x, y, z, yaw):
        ENU_x = x * math.cos(yaw) + y * math.sin(yaw)
        ENU_y = x * math.sin(yaw) - y * math.cos(yaw)
        ENU_z = z
        return ENU_x, ENU_y, ENU_z

    # q -> yaw(rad)
    def q2yaw(self, qw, qx, qy, qz):
        error = np.sqrt(abs(qw ** 2 + qx ** 2 + qy ** 2 + qz ** 2 - 1))
        if error > 0.03:
            print ('Sum of squares of q is not 1')
        siny_cosp = 2 * (float(qw) * float(qz) + float(qx) * float(qy))
        cosy_cosp = 1 - 2 * (float(qy) * float(qy) + float(qz) * float(qz))
        yaw = math.atan2(siny_cosp, cosy_cosp)
        return yaw

    # rpy(rad) -> q
    def RPY2Quar(self, roll=0.0, pitch=0.0, yaw=0.0):
        cy = math.cos(yaw * 0.5)
        sy = math.sin(yaw * 0.5)
        cp = math.cos(pitch * 0.5)
        sp = math.sin(pitch * 0.5)
        cr = math.cos(roll * 0.5)
        sr = math.sin(roll * 0.5)
        q_list = quaternion_from_euler(roll, pitch, yaw)
        q = Quaternion()
        # q.w = cy * cp * cr + sy * sp * sr
        # q.x = cy * cp * sr - sy * sp * cr
        # q.y = sy * cp * sr + cy * sp * cr
        # q.z = sy * cp * cr - cy * sp * sr
        q.x = q_list[0]
        q.y = q_list[1]
        q.z = q_list[2]
        q.w = q_list[3]
        return q

    # <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< functional function  >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
    # set target format to PoseStamped
    def construct_target_PoseStamped(self, x, y, z, qw=1.0, qx=0.0, qy=0.0, qz=0.0):
        target_raw_pose = PoseStamped()
        target_raw_pose.header.stamp = rospy.Time.now()
        target_raw_pose.pose.position.x = x
        target_raw_pose.pose.position.y = y
        target_raw_pose.pose.position.z = z
        target_raw_pose.pose.orientation.x = qx
        target_raw_pose.pose.orientation.y = qy
        target_raw_pose.pose.orientation.z = qz
        target_raw_pose.pose.orientation.w = qw
        return target_raw_pose

    def construct_target_PositionTarget(self, x=None, y=None, z=None, vx=None, vy=None, vz=None, ax=None, ay=None,
                                        az=None, F=None, yaw=None, yaw_rate=None, frame=None):
        target_raw_pose = PositionTarget()

        # frame coordinate
        # print frame
        if frame == "local_frame" or frame == 1:
            target_raw_pose.coordinate_frame = 1  # body frame: 8;  local frame : 1
        elif frame == "body_frame" or frame == 2:
            target_raw_pose.coordinate_frame = 8
        else:
            print ('frame input is wrong,set frame be local_frame')
            target_raw_pose.coordinate_frame = 1

        # calculate type mask, but can only control (xyz) or (vx vy vz)  +  (yaw or yaw rate)
        L = [x, y, z, vx, vy, vz, ax, ay, az, F, yaw, yaw_rate]
        s = 0
        for i in range(len(L)):
            if L[i] == None:
                s = s + 2 ** i
        target_raw_pose.header.stamp = rospy.Time.now()
        target_raw_pose.type_mask = s
        if x is not None:
            target_raw_pose.position.x = float(x)
        if y is not None:
            target_raw_pose.position.y = float(y)
        if z is not None:
            target_raw_pose.position.z = float(z)
        if vx is not None:
            target_raw_pose.velocity.x = float(vx)
        if vy is not None:
            target_raw_pose.velocity.y = float(vy)
        if vz is not None:
            target_raw_pose.velocity.z = float(vz)
        if ax is not None:
            target_raw_pose.acceleration_or_force.x = ax
        if ay is not None:
            target_raw_pose.acceleration_or_force.y = ay
        if az is not None:
            target_raw_pose.acceleration_or_force.z = az
        if yaw is not None:
            target_raw_pose.yaw = float(yaw)
        if yaw_rate is not None:
            target_raw_pose.yaw_rate = float(yaw_rate)
        return target_raw_pose

    def construct_target_AttitudeTarget(self, q, thrust=0.5):
        target_attitude_pose = AttitudeTarget()
        target_attitude_pose.header.stamp = rospy.Time.now()
        target_attitude_pose.type_mask = 7
        target_attitude_pose.orientation = q
        target_attitude_pose.thrust = thrust
        return target_attitude_pose

    # calculate the error to target
    def cal_error(self, cur_p, target_p, threshold=0.1):
        delta_x = math.fabs(cur_p[0] - target_p[0])
        delta_y = math.fabs(cur_p[1] - target_p[1])
        delta_z = math.fabs(cur_p[2] - target_p[2])
        error = np.sqrt(delta_x ** 2 + delta_y ** 2 + delta_z ** 2)
        # print ("error", error)
        if error < threshold:
            return True
        else:
            return False

    # calculate the distance to target
    def position_distance(self, cur_p, target_p, threshold=0.1):
        delta_x = math.fabs(cur_p.pose.position.x - target_p.position.x)
        delta_y = math.fabs(cur_p.pose.position.y - target_p.position.y)
        delta_z = math.fabs(cur_p.pose.position.z - target_p.position.z)
        error = np.sqrt(delta_x ** 2 + delta_y ** 2 + delta_z ** 2)
        # print ("error", error)
        if error < threshold:
            return True
        else:
            return False

    # calculate the vertical error
    def position_vertical_error(self, cur_p, target_p, yaw, threshold=0.1):
        delta_x = target_p.position.x - cur_p.pose.position.x
        delta_y = target_p.position.y - cur_p.pose.position.y
        delta_z = target_p.position.z - cur_p.pose.position.z
        error_f, error_l, error_u = self.ENU2FLU(delta_x, delta_y, delta_z, yaw)
        self.forward_error = abs(error_f)
        # vertical_error = np.sqrt(error_l**2 + error_u**2)
        vertical_error = max(abs(error_l), abs(error_u))
        # print ("vertical error", vertical_error)
        if vertical_error < threshold:
            return True
        else:
            return False

    # calculate horizontal_error
    def position_horizontal_error(self, cur_p, target_p, threshold=0.1):
        delta_x = target_p[0] - cur_p[0]
        delta_y = target_p[1] - cur_p[1]
        horizontal_error = max(abs(delta_x), abs(delta_y))
        # print ("vertical error", vertical_error)
        if horizontal_error < threshold:
            return True
        else:
            return False

    # calculate the fly break error in pv control
    def cal_pv_fly_error(self, cur_p, target_p, yaw, threshold=0.1):
        delta_x = target_p[0] - cur_p.pose.position.x
        delta_y = target_p[1] - cur_p.pose.position.y
        delta_z = target_p[2] - cur_p.pose.position.z
        error_f, error_l, error_u = self.ENU2FLU(delta_x, delta_y, delta_z, yaw)
        self.forward_error = error_f
        # vertical_error = np.sqrt(error_l**2 + error_u**2)
        if error_f < 0:
            # print('error_f',error_f)
            return True
        else:
            return False

    # calculate the fly break error in pv control
    def cal_forward_error(self, cur_p, target_p, yaw):
        delta_x = target_p[0] - cur_p.pose.position.x
        delta_y = target_p[1] - cur_p.pose.position.y
        delta_z = target_p[2] - cur_p.pose.position.z
        error_f, error_l, error_u = self.ENU2FLU(delta_x, delta_y, delta_z, yaw)
        return error_f
